#include <cmath>
#inlcude <vector>

namespace Reflector
{
    // 设定反光柱检测的最小激光强度
    const double REFLECTOR_MIN_INTENSITY = 1000.0f;
    // 设定反光柱半径-0.05m（即 5 cm）
    const double REFLECTOR_RADIUS = 0.05f;
    // 设定反光柱聚类的最小点数
    const double REFLECTOR_POINTS_NUM = 8;
    // 反光柱之间最小间距
    const double REFLECTOR_MIN_DISTANCE = 0.5;
    // 反光柱之间的最大间距
    const double REFLECTOR_MAX_DISTANCE = 30;
    // 反光柱距离差异的阈值
    const double REFLECTOR_MATCH_THRES = 0.5;

    // 设定激光数据结构
    struct LaserData 
    {
        int index; // 点在激光中的索引号
        float range; // 激光返回的距离
        float intensity; // 激光强度
        float x; // 在 laser 坐标系下的笛卡尔坐标
        float y; // 在 laser 坐标系下的笛卡尔坐标
    };

    // 设定反光柱的数据结构
    struct ReflectorPost
    {
        int index; // 反光柱的编号，必须在整个建图过程中不发生改变，因此要进行匹配
        float x; // 反光柱在 laser 坐标系下的笛卡尔坐标
        float y; // 反光柱在 laser 坐标系下的笛卡尔坐标
        std::set<double> distance_signature; // 到其他反光柱距离
    };

    class ReflectorDetector: public rclcpp::Node
    {
        public: 
        // 构造函数，初始化节点
        ReflectorDetector(): Node("reflector_detector") ;
        
        private:
        // 获得当前反光柱与其他反光柱之间的距离
        std::set<double> GetDistanceSignature(std::pair<double, double>& point,
                                              std::vector<std::pair<double, double>>& points);                               

        // 获得当前反光柱的距离签名和预先安排的反光柱两两之间的距离签名之间的匹配度
        double GeMatchSignature( std::set<double> & sg1,
                                 std::set<double> & sg2);

        // 为每个反光柱分配一个稳定的ID，该ID在全局中保持唯一
        void AssignStableIds(std::vector<std::pair<double, double>>& current_positions, 
                             std::vector<std::string>& assigned_ids);
        // 激光雷达接受回调
        void LaserCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan);

        // 变量
        std::pair<double, double> pre_positions;
        std::vector<cartographer_ros_msgs::LandmarkList> landmark_list;
        int next_id;
    } 
}
